#include "BeltSystem.h"

namespace Team675
{

BeltSystem::BeltSystem()
{
	m_pickupMotor = new Victor(BELT_MOTOR_PICKUP);
	m_shootingMotor = new Victor(BELT_MOTOR_SHOOTING);
	
	m_beltFirstRun = false;
	m_beltDump = false;
	
	m_pickupControl = false;
	m_pickupTime.Reset();
	
	m_shootTime.Reset();
}

void BeltSystem::Update()
{
	int t_pickupSetting = 0;
	int t_shootSetting = 0;
	
	if(m_pickupControl) t_pickupSetting = BELT_PICKUP_FORWARD;
	
	if(m_shootTime.Get() < BELT_SHOOT_TIME && m_beltFirstRun)
	{
		if(m_pickupTime.Get() < BELT_PICKUP_TIME) t_pickupSetting = BELT_PICKUP_REVERSE;
		else t_pickupSetting = 0;
		
		t_shootSetting = BELT_SHOOT_FORWARD;
	}
	
	if(m_beltDump)
	{
		t_pickupSetting = BELT_PICKUP_REVERSE;
		t_shootSetting = BELT_SHOOT_REVERSE;
	}
	
	m_pickupMotor->Set((float) t_pickupSetting * BELT_PICKUP_SPEED);
	m_shootingMotor->Set((float) t_shootSetting * BELT_SHOOT_SPEED);
	
	m_pickupControl = false;
	m_beltDump = false;
	
	if(DEBUG_MODE)
	{
		SmartDashboard::GetInstance()->Log(t_pickupSetting, "Pickup Belt");
		SmartDashboard::GetInstance()->Log(t_shootSetting, "Shooting Belt");
	}
	else
	{
		switch(t_pickupSetting)
		{
		case BELT_PICKUP_FORWARD:
			SmartDashboard::GetInstance()->Log("Sucking", "Pickup Belt");
		break;
		
		case BELT_PICKUP_REVERSE:
			SmartDashboard::GetInstance()->Log("Spitting", "Pickup Belt");
		break;
		
		default:
			SmartDashboard::GetInstance()->Log("Off", "Pickup Belt");
		break;
		}
		
		if(t_shootSetting == BELT_SHOOT_FORWARD)
			SmartDashboard::GetInstance()->Log("Shooting", "Shooting Belt");
		else
			SmartDashboard::GetInstance()->Log("Off", "Shooting Belt");
	}
}

void BeltSystem::Pickup()
{
	m_pickupControl = true;
}

void BeltSystem::Shoot()
{
	if(m_shootTime.Get() > BELT_SHOOT_TIME || !m_beltFirstRun)
	{
		m_beltFirstRun = true;
		
		m_pickupTime.Reset();
		m_pickupTime.Start();
		m_shootTime.Reset();
		m_shootTime.Start();
	}
}

void BeltSystem::Dump()
{
	m_beltDump = true;
}

}
